import cv2
import numpy as np
import serial

# img=cv2.imread('bend.jpg')
# img_hsv=cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
# cv2.imshow('img',img_hsv)

ser = serial.Serial("/dev/ttyAMA0", 9600)

# 赛道边界灰色阈值
lower = np.array([10, 30, 46])
upper = np.array([180, 43, 150])
capL = cv2.VideoCapture(2)  # 左侧摄像头
capR = cv2.VideoCapture(3)  # 右侧摄像头

capL.set(3, 320)
capL.set(4, 240)

capR.set(3, 320)
capR.set(4, 240)

while 1:
    retL, frameL = capL.read()
    retR, frameR = capR.read()
    frameL = cv2.GaussianBlur(frameL, (5, 5), 0)
    frameR = cv2.GaussianBlur(frameR, (5, 5), 0)
    hsvL = cv2.cvtColor(frameL, cv2.COLOR_BGR2HSV)
    hsvR = cv2.cvtColor(frameR, cv2.COLOR_BGR2HSV)
    maskL = cv2.inRange(hsvL, lower, upper)
    maskR = cv2.inRange(hsvR, lower, upper)
    maskL = cv2.erode(maskL, None, iterations=2)
    maskR = cv2.erode(maskR, None, iterations=2)
    maskL = cv2.GaussianBlur(maskL, (3, 3), 0)
    maskR = cv2.GaussianBlur(maskR, (3, 3), 0)
    resL = cv2.bitwise_and(frameL, frameL, mask=maskL)
    resR = cv2.bitwise_and(frameR, frameR, mask=maskR)

    cntsL = cv2.findContours(
        maskL.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]

    cntsR = cv2.findContours(
        maskR.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]

    if len(cntsL) > 0:
        cnt = max(cntsL, key=cv2.contourArea)
        (x, y), radius = cv2.minEnclosingCircle(cnt)
        cv2.circle(frameL, (int(x), int(y)), int(radius), (255, 0, 255), 2)
        # print(int(x), int(y))
        flagL = '1'
        print(flagL)
        ser.write(flagL.encode())
    else:
        pass

    cv2.imshow('frameLeft', frameL)
    cv2.imshow('hsvLeft', hsvL)
    if len(cntsR) > 0:
        cnt = max(cntsR, key=cv2.contourArea)
        (x, y), radius = cv2.minEnclosingCircle(cnt)
        cv2.circle(frameR, (int(x), int(y)), int(radius), (255, 0, 255), 2)
        # print(int(x), int(y))
        flagR = '2'
        print(flagR)
        ser.write(flagR.encode())
    else:
        pass
    cv2.imshow('frameRight', frameR)
    cv2.imshow('hsvRight', hsvR)

    if cv2.waitKey(5) & 0xFF == 27:
        break

capL.release()
cv2.destroyAllWindows()
